#include "glbase/glflat.h"
#include "glbase/udpwin32.h"

Socket udp;
#pragma pack(push,1) // turn off data boundary alignment
typedef struct _DataStruct
{
	ULONG loops;
	float runtime;
	float sample;
	float a[3];
	float v[3];
	float xyz[3];
	float lla[3];
	float w[3];
	float euler[3];
} DataStruct;
#pragma pack(pop)
const int dataSize=sizeof(DataStruct);
List<DataStruct *> dataQueue;

class TestXPlane:public FlatApplication
{
protected:
	BOOL initOK;
	Thread listenProcess;
	DataStruct *data,simData;
	float xyz[3],xyz2[3],v[3],v2[3],a[3];
	BOOL simError;
	BOOL create();
	BOOL glCreate();
	void glDestroy();
	void destroy();
	void reshape ();
	BOOL listen(WORD id, WORD code, const void *info);
	void update();
	void draw2D();
	void draw3D();
	void simStep();
} testXPlane;

DWORD WINAPI listenProc(void* param)
{
	for(;;)
	{
		DataStruct *data=new DataStruct;
		if(!udp.receive((char *)data,dataSize))
		{
			delete data;
			break;
		}
		else
		{
			dataQueue.add(data);
			testXPlane.wake();
		}
	}
	return 0;
}

BOOL TestXPlane::create ()				
{
	FlatApplication::create();
	name=_T("Test X-Plane");
	display.width=800;
	display.height=600;
	display.bpp=32;
	data=NULL;
	simError=FALSE;
	initOK=FALSE;
	ClearMemory(&simData,dataSize);
	udp.set("localhost",1500,3000,false);
	if(!udp.error)
		udp.open();
	listenProcess.procedure=listenProc;
	listenProcess.restart();
	return TRUE;
}
BOOL TestXPlane::glCreate ()
{
	FlatApplication::glCreate();
	return TRUE;
}
void TestXPlane::glDestroy()
{
	FlatApplication::glDestroy();
}
void TestXPlane::destroy()
{
	FlatApplication::destroy();
	ListObject<DataStruct *> *obj;
	for(obj=dataQueue.first;obj;obj=obj->next)
		delete obj->value;
}
void TestXPlane::reshape()
{
	FlatApplication::reshape();
}
BOOL TestXPlane::listen(WORD id, WORD code, const void *info)
{
	return FALSE;
}
void TestXPlane::update()
{
	if(keyboard.keys[VK_F5])
		initOK=FALSE;
	if(dataQueue.length)
	{
		updated=TRUE;
		data=dataQueue.first->value;
		simStep();
		delete data;
		data=NULL;
		dataQueue.remove(0);
	}
}
void TestXPlane::draw2D()
{
	font.color=INT_WGRAY;
	font.x=10;
	font.y=20;
	glPrint(&font,window.title);
	font.color=INT_WHITE;
	font.y+=18;
	glPrint(&font,_T("UDP error: %s"),boolToStr(udp.error));
	font.y+=18;
	glPrint(&font,_T("IP: %s"),udp.getRemoteIP());
	font.y+=18;
	glPrint(&font,_T("listen port: %d"),udp.localport);
	font.y+=18;
	glPrint(&font,_T("send port: %d"),udp.remoteport);
	font.y+=18;
	glPrint(&font,_T("Listen error: %s"),boolToStr(listenProcess.error));
	font.y+=18;
	glPrint(&font,_T("Listen return: %d"),listenProcess.getExitCode());
	font.y+=18;
	glPrint(&font,_T("Listen complete: %s"),boolToStr(listenProcess.isReady()));
	font.y+=18;
	glPrint(&font,_T("Sim error: %s"),boolToStr(simError));
	font.y+=18;
	glPrint(&font,_T("Sim loop: %d"),simData.loops);
	font.y+=18;
	glPrint(&font,_T("Sim runtime: %f"),simData.runtime);
	font.y+=18;
	glPrint(&font,_T("Sim sample: %f"),simData.sample);
	font.y+=18;
	glPrint(&font,_T("Sim Ax: %12f"),simData.a[0]);
	font.y+=18;
	glPrint(&font,_T("Sim Ay: %12f"),simData.a[1]);
	font.y+=18;
	glPrint(&font,_T("Sim Az: %12f"),simData.a[2]);
	font.y+=18;
	glPrint(&font,_T("Sim Vx: %12f, Calc Vx: %12f, Error Vx: %12f"),simData.v[0],v[0],v[0]-simData.v[0]);
	font.y+=18;
	glPrint(&font,_T("Sim Vy: %12f, Calc Vy: %12f, Error Vy: %12f"),simData.v[1],v[1],v[1]-simData.v[1]);
	font.y+=18;
	glPrint(&font,_T("Sim Vz: %12f, Calc Vz: %12f, Error Vz: %12f"),simData.v[2],v[2],v[2]-simData.v[2]);
	font.y+=18;
	glPrint(&font,_T("Sim x:  %12f, Calc x:  %12f, Calc x2:  %12f"),simData.xyz[0],xyz[0],xyz2[0]);
	font.y+=18;
	glPrint(&font,_T("Sim y:  %12f, Calc y:  %12f, Calc y2:  %12f"),simData.xyz[1],xyz[1],xyz2[0]);
	font.y+=18;
	glPrint(&font,_T("Sim z:  %12f, Calc z:  %12f, Calc z2:  %12f"),simData.xyz[2],xyz[2],xyz2[0]);
	font.y+=18;
	glPrint(&font,_T("Error x:  %12f, Error x2:  %12f"),xyz[0]-simData.xyz[0],xyz2[0]-simData.xyz[0]);
	font.y+=18;
	glPrint(&font,_T("Error y:  %12f, Error y2:  %12f"),xyz[1]-simData.xyz[1],xyz2[1]-simData.xyz[1]);
	font.y+=18;
	glPrint(&font,_T("Error z:  %12f, Error z2:  %12f"),xyz[2]-simData.xyz[2],xyz2[2]-simData.xyz[2]);
}
void TestXPlane::draw3D()
{
}
void TestXPlane::simStep()
{
	if(simError)
		return;
	if(simData.loops>data->loops)
	{
		simError=TRUE;
		return;
	}
	memcpy(&simData,data,dataSize);
	if(!initOK)
	{
		xyz2[0]=xyz[0]=simData.xyz[0];
		xyz2[1]=xyz[1]=simData.xyz[1];
		xyz2[2]=xyz[2]=simData.xyz[2];
		v2[0]=v[0]=simData.v[0];
		v2[1]=v[1]=simData.v[1];
		v2[2]=v[2]=simData.v[2];
		a[0]=simData.a[0];
		a[1]=simData.a[1];
		a[2]=simData.a[2];
		initOK=TRUE;
	}
	else
	{
		xyz[0]+=v[0]*simData.sample;
		xyz[1]+=v[1]*simData.sample;
		xyz[2]+=v[2]*simData.sample;
		xyz2[0]+=v2[0]*simData.sample;
		xyz2[1]+=v2[1]*simData.sample;
		xyz2[2]+=v2[2]*simData.sample;
		v[0]+=a[0]*simData.sample;
		v[1]+=a[1]*simData.sample;
		v[2]+=a[2]*simData.sample;
		v2[0]=simData.v[0];
		v2[1]=simData.v[1];
		v2[2]=simData.v[2];
		a[0]=simData.a[0];
		a[1]=simData.a[1];
		a[2]=simData.a[2];
	}
}

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	return testXPlane.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}